using System;
using System.Collections.Generic;
using System.Collections;
using System.Text;
using Blimpbots.Recognition;
using Blimpbots.Recognition.Algorithms;
using Blimpbots.Control;
using DotNetMatrix;
using System.Runtime.Remoting;
using System.Runtime.Remoting.Channels;
using System.Runtime.Remoting.Channels.Tcp;
using System.Runtime.Remoting.Services;
using Blimpbots.Recognition.Remoting;


namespace Blimpbots.Recognition
{

    #region TrackingServerEventArgs

    public delegate void ServerEventHandler(object sender, TrackingServerEventArgs e);

    public class TrackingServerEventArgs : EventArgs
    {        
        private List<Line2D> _lines;
        private ArrayList _results;
        public TrackingServerEventArgs(ArrayList results, List<Line2D> lines)
        {
            _lines = lines;
            _results = results;
        }

        public List<Line2D> Lines { get { return _lines; } }
        public ArrayList Results { get { return _results; } }

    }

    #endregion

    public struct BlimpState
    {
        public string BlimpName;
        public Point3D FilteredPosition;
        public double FilteredOrientation;
        public Point3D LastRawPosition;
        public DateTime LastDetection;
        public double Voltage;
    }

    public class BlimpFilters
    {
        public PointMassKalmanFilter kf;
        public SimpleAngleFilter saf;
        public BlimpState curState;
        private DateTime startTime;

        System.IO.StreamWriter sw;

        public BlimpFilters(string name, DateTime startTime)
        {
            sw = new System.IO.StreamWriter(name + ".csv", false);

            this.startTime = startTime;
            curState.BlimpName = name;
            kf = new PointMassKalmanFilter(50, 50, 50, 1000 * 50,
                new Point3D(0, 0, 0), 0);
            saf = new SimpleAngleFilter(0, 6.28);
        }

        public void Update(EstimatedPoint center, GeneralMatrix ellipseRot, double oriXY, bool oriGood, double confidence, DateTime t)
        {
            curState.LastDetection = t;
            curState.LastRawPosition = center.Position;

            // Filter the position
            double timeSec = ((TimeSpan)t.Subtract(startTime)).TotalSeconds;
            GeneralMatrix z = center.Position.ToMatrix();

            // Add error from the confidence term
            //   currently, confidence is the 3D euclidian distance (mm)
            //   between distorted and undistorted reconstructions.
            // 80/80/260 for a table on the atrium floor.

            double errStDevX = 100 + (center.Confidence / 2);
            double errStDevY = 100 + (center.Confidence / 2);
            double errStDevZ = 300 + (center.Confidence / 2);
        
            //double errStDevX = 300 + (center.Confidence / 2);
            //double errStDevY = 300 + (center.Confidence / 2);
            //double errStDevZ = 300 + (center.Confidence / 2);
            
            Point3D est = kf.Update(center.Position, errStDevX, errStDevY, errStDevZ, ellipseRot, timeSec);

            sw.WriteLine("{0},{1},{2}", est.X, est.Y, est.Z);
            sw.Flush();

            // Some way of representing the angular accuracy
            double angStd = 0.5 + (1.0 - confidence) * 6.28;

            //if (oriGood)
            //{
                // Filter the orientation (XY)
                saf.Update(oriXY, angStd);
            //}
        }

    }

    class Server
    {
        private DateTime startTime;
        private Remoting.cTransfer mi_Transfer = null;
        private ObjRef mi_Service = null;
        private TcpChannel mi_Channel = null;
        public GlobalBlimpControl controller = null;
        Dictionary<string, BlimpFilters> blimpFilters = new Dictionary<string, BlimpFilters>();

        /// <summary>
        /// Estimates the position for all currently-tracked objects.
        /// </summary>
        /// <returns></returns>
        public BlimpState[] GetCurrentState()
        {
            double timeSec = ((TimeSpan)DateTime.Now.Subtract(startTime)).TotalSeconds;

            BlimpState[] state = new BlimpState[blimpFilters.Count];
            int i = 0;
            foreach (KeyValuePair<string, BlimpFilters> kvp in blimpFilters)
            {
                BlimpFilters bl = kvp.Value;

                state[i].Voltage = controller.ReadVoltage(kvp.Key);

                // Update estimates
                bl.curState.FilteredPosition = bl.kf.Predict(timeSec);
                bl.curState.FilteredOrientation = bl.saf.Predict(timeSec);
                
                state[i] = kvp.Value.curState; i++; 
            }

            return state;
        }

        public Server(bool disableBlimps, out double xOffset, out double xFactor, out double yOffset, out double yFactor)
        {
            startTime = DateTime.Now;
            double roomXmin, roomXmax, roomYmin, roomYmax;

            // these should be the only numbers that actually care about the correspondence of the blimp coordinates to the real world
            // (along with the coordinates in the svg file)
            double blimpLength = 1700;
            double blimpWidth = 475;
            double blimpHeight = 700;

            Point2D roomCenter = new Point2D(2750, 5000);

            double zSetPoint = 2600;

            controller = new GlobalBlimpControl(disableBlimps, blimpLength, blimpWidth, blimpHeight, zSetPoint, roomCenter,
                                                out roomXmin, out roomXmax, out roomYmin, out roomYmax);

            double GUIwidth = 495;
            double GUIheight = 422;
            roomXmax += 1000; roomYmax += 1000; roomYmin -= 1000; roomXmin -= 1000;
            xOffset = -1*roomXmin;
            yOffset = -1*roomYmin;

            xFactor = GUIwidth / (roomXmax - roomXmin);
            yFactor = GUIheight / (roomYmax - roomYmin);

        }

        public void Update()
        {

            BlimpState[] stateArr = GetCurrentState();

            foreach (BlimpState state in stateArr)
            {                
                controller.updateBlimp(state.BlimpName, state.FilteredPosition, state.FilteredOrientation, state.LastDetection);
            }

            latestLines.Clear();
            latestLines.AddRange(controller.getWorldLines());
            latestLines.AddRange(controller.getBlimpLines());
            latestState = stateArr;
        }

        public BlimpState[] latestState = new BlimpState[0];
        public List<Line2D> latestLines = new List<Line2D>();

        public void StopAllMotors()
        {
            controller.stopAll();
        }

        public void StartListen()
        {
            StopListen(); // if there is any channel still open --> close it

            try
            {
                int s32_Port = 12345; // port 12345

                mi_Channel = new TcpChannel(s32_Port);
                ChannelServices.RegisterChannel(mi_Channel, false);

                mi_Transfer = new Remoting.cTransfer();
                mi_Service = RemotingServices.Marshal(mi_Transfer, "TestService");

                // define the event which is triggered when the Master calls the CallSlave() function
                mi_Transfer.ev_SlaveCall += new Remoting.cTransfer.del_SlaveCall(OnMasterEvent);
            }
            catch (Exception Ex)
            {
                Console.WriteLine("Error starting listening:\n" + Ex.Message, "Slave");
            }
        }
        public void StopListen()
        {
            if (mi_Service != null)
                RemotingServices.Unmarshal(mi_Service);

            if (mi_Transfer != null)
                RemotingServices.Disconnect(mi_Transfer);

            if (mi_Channel != null)
                ChannelServices.UnregisterChannel(mi_Channel);

            mi_Service = null;
            mi_Transfer = null;
            mi_Channel = null;

        }

        private int TryFindPointIndex(string pointName, EstimatedPoint[] ptArray)
        {
            return Array.FindIndex<EstimatedPoint>(ptArray,
                delegate(EstimatedPoint pt)
                {
                    return pt.Name == pointName;
                });
        }
        

        private enum OrientationPlane
        {
            XZ, XY, YZ
        }

        double FindOrientation(OrientationPlane plane, Point3D front, Point3D center)
        {
            double dim1_dist = 0, dim2_dist = 0;
            if (plane == OrientationPlane.XZ)
            {
                dim1_dist = front.X - center.X;
                dim2_dist = front.Z - center.Z;
            }
            else if (plane == OrientationPlane.XY)
            {
                dim1_dist = front.X - center.X;
                dim2_dist = front.Y - center.Y;
            }

            return Math.Atan2(dim2_dist, dim1_dist);
        }


        BlimpFilters GetBlimpFilters(string name)
        {
            BlimpFilters filters = null;

            if (!blimpFilters.TryGetValue(name, out filters))
            {
                filters = new BlimpFilters(name, startTime);
                blimpFilters.Add(name, filters);
            }

            return filters;
        }

        Remoting.kResponse OnMasterEvent(Remoting.kAction k_Action)
        {
            foreach (Remoting.blimpData bd in k_Action.blimps) { 

                int center_idx = TryFindPointIndex("center", bd.estPts);
                int front_idx = TryFindPointIndex("front", bd.estPts);

                double oriXY = 0;

                if (center_idx != -1 && front_idx != -1)
                {
                    EstimatedPoint front = bd.estPts[front_idx];
                    EstimatedPoint center = bd.estPts[center_idx];
                    oriXY = FindOrientation(OrientationPlane.XY, front.Position,
                                            center.Position);

                    GeneralMatrix ellipseRot = new GeneralMatrix(center.EllipseRotation, 3);

                    BlimpFilters filters = GetBlimpFilters(bd.name);
                    filters.Update(center, ellipseRot, oriXY, bd.angleGood, bd.confidence, k_Action.t);
                }
            }
            Remoting.kResponse k_Response = new Remoting.kResponse();
            k_Response.s_Result = "OK";
            return k_Response;
        }
    }
}
